
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_gps.h
  * @author     baiyang
  * @date       2022-2-16
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>

#include <rtconfig.h>

#include "sensor_gps_backend.h"
#include "sensor_gps_detect_state.h"

#include <common/gp_config.h>
/*-----------------------------------macro------------------------------------*/
/**
   maximum number of GPS instances available on this platform. If more
   than 1 then redundant sensors may be available
 */
#ifndef GPS_MAX_RECEIVERS
#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
#endif
#if !defined(GPS_MAX_INSTANCES)
#if GPS_MAX_RECEIVERS > 1
#define GPS_MAX_INSTANCES  (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data
#else
#define GPS_MAX_INSTANCES 1
#endif // GPS_MAX_RECEIVERS > 1
#endif // GPS_MAX_INSTANCES

#if GPS_MAX_RECEIVERS <= 1 && GPS_MAX_INSTANCES > 1
#error "GPS_MAX_INSTANCES should be 1 for GPS_MAX_RECEIVERS <= 1"
#endif

#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS
#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS  // the virtual blended GPS is always the highest instance (2)
#endif
#define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink

// the number of GPS leap seconds - copied into SIM_GPS.cpp
#define GPS_LEAPSECONDS_MILLIS 18000ULL

#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * GP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)

#if (!defined(HAL_MINIMIZE_FEATURES) && GPS_MAX_RECEIVERS>1)
#define GPS_MOVING_BASELINE 1
#else
#define GPS_MOVING_BASELINE 0
#endif

#ifndef HAL_MSP_GPS_ENABLED
//#define HAL_MSP_GPS_ENABLED HAL_MSP_SENSORS_ENABLED
#define HAL_MSP_GPS_ENABLED 0
#endif

/*----------------------------------typedef-----------------------------------*/
enum SBAS_Mode {
    SBAS_Disabled = 0,
    SBAS_Enabled = 1,
    SBAS_DoNotChange = 2,
};

struct GPS_timing {
    // the time we got our last fix in system milliseconds
    uint32_t last_fix_time_ms;

    // the time we got our last message in system milliseconds
    uint32_t last_message_time_ms;

    // delta time between the last pair of GPS updates in system milliseconds
    uint16_t delta_time_ms;

    // count of delayed frames
    uint8_t delayed_count;

    // the average time delta
    float average_delta_ms;
};

// state of auto-detection process, per instance
struct gps_detect_state {
    uint32_t last_baud_change_ms;
    uint8_t current_baud;
    bool auto_detected_baud;
    struct UBLOX_detect_state ublox_detect_state;
    struct SIRF_detect_state sirf_detect_state;
    struct NMEA_detect_state nmea_detect_state;
    struct SBP_detect_state sbp_detect_state;
    struct SBP2_detect_state sbp2_detect_state;
    struct ERB_detect_state erb_detect_state;
};

/** @ 
  * @brief  
  */
typedef struct {
    // configuration parameters
    Param_int8 _type[GPS_MAX_RECEIVERS];
    Param_int8 _navfilter;
    Param_int8 _auto_switch;
    Param_int8 _min_dgps;
    Param_int16 _sbp_logmask;
    Param_int8 _inject_to;
    Param_int8 _sbas_mode;
    Param_int8 _min_elevation;
    Param_int8 _raw_data;
    Param_int8 _gnss_mode[GPS_MAX_RECEIVERS];
    Param_int16 _rate_ms[GPS_MAX_RECEIVERS];   // this parameter should always be accessed using get_rate_ms()
    Param_int8 _save_config;
    Param_int8 _auto_config;
    Param_Vector3f _antenna_offset[GPS_MAX_RECEIVERS];
    Param_int16 _delay_ms[GPS_MAX_RECEIVERS];
    Param_int8  _com_port[GPS_MAX_RECEIVERS];
    Param_int8 _blend_mask;
    Param_float _blend_tc;
    Param_int16 _driver_options;
    Param_int8 _primary;

    const uint32_t  *_baudrates;
    const char *_initialisation_blob;

    uint32_t _last_instance_swap_ms;

    struct gps_detect_state detect_state[GPS_MAX_RECEIVERS];

    // Note allowance for an additional instance to contain blended data
    struct GPS_timing timing[GPS_MAX_INSTANCES];
    struct GPS_State state[GPS_MAX_INSTANCES];
    sensor_gps_backend_t drivers[GPS_MAX_RECEIVERS];
    rt_device_t _port[GPS_MAX_RECEIVERS];

    /// primary GPS instance
    uint8_t primary_instance;

    /// number of GPS instances present
    uint8_t num_instances;

    // which ports are locked
    uint8_t locked_ports;

    struct {
        const char *blob;
        uint16_t remaining;
    } initblob_state[GPS_MAX_RECEIVERS];
 
    // GPS blending and switching
    Vector3f_t _blended_antenna_offset; // blended antenna offset
    float _blended_lag_sec; // blended receiver lag in seconds
    float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
    float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets
    bool _output_is_blended; // true when a blended GPS solution being output
    uint8_t _blend_health_counter;  // 0 = perfectly health, 100 = very unhealthy

    // used for flight testing with GPS loss
    bool _force_disable_gps;

    // used for flight testing with GPS yaw loss
    bool _force_disable_gps_yaw;
} sensor_gps;

/*----------------------------------variable----------------------------------*/
extern sensor_gps gps;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_gps_ctor();
sensor_gps* sensor_gps_get_singleton();
void sensor_gps_init();
void sensor_gps_assign_param();
uint16_t sensor_gps_get_rate_ms(uint8_t instance);
bool sensor_gps_get_lag(uint8_t instance, float *lag_sec);
bool sensor_gps_first_unconfigured_gps(uint8_t *instance);
enum GPS_Type sensor_gps_get_type(uint8_t instance);
void sensor_gps_send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
void sensor_gps_send_blob_update(uint8_t instance);
void sensor_gps_update(void);
#if GPS_MAX_RECEIVERS > 1
void sensor_gps_update_primary(void);
#endif
void sensor_gps_update_instance(uint8_t instance);
void sensor_gps_detect_instance(uint8_t instance);
void sensor_gps_inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
uint64_t sensor_gps_time_epoch_usec(uint8_t instance);
uint64_t sensor_gps_time_epoch_convert(uint16_t gps_week, uint32_t gps_ms);
bool sensor_gps_is_healthy(uint8_t instance);
bool sensor_gps_blend_health_check();
uint8_t sensor_gps_num_sensors(void);
struct GPS_State* sensor_gps_primary_state();
struct GPS_timing* sensor_gps_primary_timing();
bool sensor_gps_backends_healthy(char failure_msg[50], uint16_t failure_msg_len);
enum GPS_Status sensor_gps_status2(uint8_t instance);
enum GPS_Status sensor_gps_status();
bool sensor_gps_all_consistent(float* distance);
const Location *sensor_gps_location2(uint8_t instance);
const Location *sensor_gps_location();
void sensor_gps_broadcast_first_configuration_failure_reason(void);
uint16_t sensor_gps_get_hdop2(uint8_t instance);
uint16_t sensor_gps_get_hdop();
bool sensor_gps_prepare_for_arming(void);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



